

#include "preprocess/map_process.h"
#include "glog_func/log.h"

#include <ros/ros.h>
#include <numeric>


namespace global_path_planner
{
    namespace preprocess
    {

        MapProcess::MapProcess(std::shared_ptr<MapData>& map_data)
                            :__map_Data_Ptr(map_data)
        {
            ros::param::param<float>("/clean_width/sweep_width", __brush_Length, 0.2);
        }

        MapProcess::~MapProcess()
        {

        }

        // 更新地图
        void MapProcess::UpdateMapImage(const nav_msgs::OccupancyGridConstPtr &grid_map,
                                        std::shared_ptr<::common::planning::PathPlan>& path_plan_ptr,
                                        const double bump_dis)
        {
            bool debug = true;
            if (grid_map == nullptr)
                return;
            auto start_time = ros::Time::now().toSec();
            __map_Grid = cv::Mat::zeros(grid_map->info.height, grid_map->info.width, CV_8UC1);
            // 地图数据转化图像
            for (int h = 0; h < grid_map->info.height; h++)
            {
                for (int w = 0; w < grid_map->info.width; w++)
                {
                    // 注意,这里依赖的是标准的地图,默认像素只要非0均不可通行
                    int idx = h * grid_map->info.width + w;
                    if (grid_map->data[idx] != 0)
                    {
                        __map_Grid.at<uchar>(h, w) = 0;
                    }
                    else
                    {
                        __map_Grid.at<uchar>(h, w) = 255;
                    }
                }
            }
            // 更新地图信息参数
            __resolution = grid_map->info.resolution;
            __origin_X = grid_map->info.origin.position.x;
            __origin_Y = grid_map->info.origin.position.y;
            __width = grid_map->info.width;
            __height = grid_map->info.height;
            // 处理虚拟墙
            // ProcessVirtueWalls(__map_Grid);

            // 调试代码
            LOG(INFO) << "to save debug grid map";
            cv::Mat tmp_map;
            cv::flip(__map_Grid, tmp_map, 0);
            std::string map_file = common::log::kGlobalPathPlannerLogPath + "/" + common::log::CurTimeString() + "_grid_map.png";
            cv::imwrite(map_file, tmp_map);
            LOG(INFO) << "to save debug grid map finish";

            // 初始化path_plan_
            path_plan_ptr->Init(std::vector<uint8_t>(__map_Grid.begin<uchar>(), __map_Grid.end<uchar>()),
                                Pose(__origin_X + 0.5 * __resolution, __origin_Y + 0.5 * __resolution),
                                __width, 
                                __height, 
                                __resolution, 
                                bump_dis,
                                bump_dis + 0.2);

            LOG(INFO) << "UpdateMapImage cost " << ros::Time::now().toSec() - start_time << " s";
        }

        // 预处理规划区域
        std::pair<int, cv::Mat> MapProcess::PreProcessPlanAreas(const cv::Mat& img, int &piexl_radius, bool &region_inflate,
                                                                std::vector<geometry_msgs::Polygon> &all_polys)
        {
            cv::Mat tmp_img = img.clone();
            std::vector<std::vector<cv::Point>> all_areas;
            // 1.加载轮廓区域
            for (auto poly : all_polys)
            {
                all_areas.emplace_back(ProcessPolygons(poly));
            }
            // 2.加载功能区
            auto func_areas = ProcessFuncAreas(); // 功能区, 当前已弃用
            for (auto func_area : func_areas)
            {
                all_areas.emplace_back(func_area.second);
            }
            // 3.规划内的障碍物进行填充
            auto plan_area = all_areas.front(); // 任务区外轮廓
            all_areas.erase(all_areas.begin());
            if (all_areas.size() > 0)
            {
                cv::fillPoly(tmp_img, all_areas, cv::Scalar(0)); // danger_areas、功能区等（当前已弃用）内部涂黑
            }
            // 4.加载最外层区域一起膨胀
            if (plan_area.size() > 0 && region_inflate)
            { // region_inflate为true, 先mask, 再腐蚀
                std::vector<std::vector<cv::Point>> vec_mask_areas;
                vec_mask_areas.emplace_back(plan_area);
                tmp_img = MaskAreaImage(tmp_img, vec_mask_areas); // 最外圈外部涂黑, 内部保留img原图
            }
            // 5.规划区域内的图像进行膨胀
            std::pair<int, cv::Mat> erode_img = PlanAreaErode(piexl_radius, tmp_img); // 腐蚀
            // 6.内部填充不可达区域
            FillNoConnectedArea(erode_img.second); // 孔洞子轮廓内部涂黑
            // 7.最外层区域不膨胀
            if (plan_area.size() > 0 && !region_inflate)
            { // region_inflate为true, 先腐蚀, 再mask
                std::vector<std::vector<cv::Point>> vec_mask_areas;
                vec_mask_areas.emplace_back(plan_area);
                erode_img.second = MaskAreaImage(erode_img.second, vec_mask_areas);
            }
            // 8.形态学闭操作, 去除狭窄白色区域 (避免findcontours内外轮廓有重合像素)
            cv::Mat open_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3), cv::Point(-1, -1));
            cv::morphologyEx(erode_img.second, erode_img.second, cv::MORPH_OPEN, open_kernel);
            return erode_img;
        }

        // 预处理功能区域
        std::vector<std::pair<int, cv::Mat>> MapProcess::PreProcessFuncAreas(cv::Mat &img)
        {
            std::vector<std::pair<int, cv::Mat>> res_mat;
            // 1.内部填充不可达区域
            FillNoConnectedArea(img);
            // 2.加载功能区轮廓
            auto func_areas = ProcessFuncAreas();
            for (auto func_area : func_areas)
            {
                std::vector<std::vector<cv::Point>> vec_mask_areas;
                vec_mask_areas.emplace_back(func_area.second);
                auto dst = MaskAreaImage(img, vec_mask_areas);
                res_mat.emplace_back(std::make_pair(func_area.first, dst));
            }
            return res_mat;
        }

        // 提取特征点
        std::unordered_map<int, std::vector<std::vector<cv::Point>>> MapProcess::GetAllContours(cv::Mat &img)
        {
            std::unordered_map<int, std::vector<std::vector<cv::Point>>> all_contours;
            // 根据膨胀查找规划区域的边界轮廓
            std::vector<std::vector<cv::Point>> cnts;
            std::vector<cv::Vec4i> hierarchy; // index: next, prev, first_child, parent
            cv::findContours(img, cnts, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
            if (cnts.size() == 0 || hierarchy.size() == 0)
            {
                return all_contours;
            }

            // 根据hierarchy[i][3] == -1, 记录父类轮廓的索引
            std::vector<int> indexs;
            for (int i = 0; i < hierarchy.size(); i++)
            {
                if (hierarchy[i][3] == -1)
                { // parent contour's index equals to external contour's index
                    indexs.emplace_back(i);
                }
            }

            // 根据父类轮廓的索引，将子类轮廓分类
            for (int i = 0; i < indexs.size(); i++)
            {
                std::vector<std::vector<cv::Point>> contours;
                contours.emplace_back(cnts[indexs[i]]);
                for (int j = 0; j < hierarchy.size(); j++)
                {
                    if (hierarchy[j][3] == indexs[i])
                    {
                        contours.emplace_back(cnts[j]);
                    }
                }
                all_contours[i] = contours;
            }
            return all_contours;
        }

        // 提取规划区域内所有的特征点
        std::map<int, std::pair<cv::Mat, std::vector<std::vector<cv::Point>>>>
        MapProcess::getAllContours(cv::Mat &img, bool& loop)
        {
            std::map<int, std::pair<cv::Mat, std::vector<std::vector<cv::Point>>>> all_mat_contours;
            // 根据膨胀查找规划区域的边界轮廓
            std::vector<std::vector<cv::Point>> cnts;
            std::vector<cv::Vec4i> hierarchy; // index: next, prev, first_child, parent
            cv::findContours(img, cnts, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
            if (cnts.size() == 0 || hierarchy.size() == 0)
            {
                return all_mat_contours;
            }

            // 根据hierarchy[i][3] == -1, 记录父类轮廓的索引
            std::vector<int> indexs;
            for (int i = 0; i < hierarchy.size(); i++)
            {
                if (hierarchy[i][3] == -1 &&
                    cnts[i].size() > 2 &&
                    cv::arcLength(cnts[i], true) >= 10.0)
                { // parent contour's index equals to external contour's index
                    indexs.emplace_back(i);
                    LOG(INFO) << "origin contour[" << i << "]'s size: " << cnts[i].size();
                }
            }
            int index = 0;
            // 根据父类轮廓的索引，将子类轮廓分类
            for (int i = 0; i < indexs.size(); i++)
            {
                cv::Mat tmp_img;
                img.copyTo(tmp_img);
                std::vector<std::vector<cv::Point>> contours;
                std::vector<std::vector<cv::Point>> approx_Contours;
                contours.emplace_back(cnts[indexs[i]]);
                for (int j = 0; j < hierarchy.size(); j++)
                {
                    if (hierarchy[j][3] == indexs[i])
                    {
                        contours.emplace_back(cnts[j]); // j为最外轮廓indexs[i]的下一级子轮廓
                    }
                    else if (hierarchy[j][3] == -1 && j != indexs[i])
                    {
                        cv::drawContours(tmp_img, cnts, j, cv::Scalar(0, 0, 0), cv::FILLED); // indexs[i]以外的其余最外轮廓内部涂黑
                    }
                }
                for (auto &contour : contours)
                {
                    std::vector<cv::Point> approx_contour;
                    if (loop) { // 非弓字任务, 该轮廓用于生成贴边/回字清洁路径
                        cv::approxPolyDP(contour, approx_contour, 0.5, true);
                    } else {
                        cv::approxPolyDP(contour, approx_contour, 1.0, true);
                    }
                    if (approx_contour.size() > 2)
                    {
                        approx_Contours.emplace_back(approx_contour);
                    }
                }
                if(approx_Contours.size() > 0) {
                    LOG(INFO) << "approx contour[" << index << "]'s size: " << approx_Contours.front().size();
                    all_mat_contours[index++] = std::make_pair(tmp_img, approx_Contours);
                }
            }
            return all_mat_contours;
        }

    // 多边形mask_area内部保留, 外部涂黑
    cv::Mat MapProcess::MaskAreaImage(cv::Mat img, std::vector<std::vector<cv::Point>> &mask_area)
    {
        cv::Mat dst;
        if (mask_area.size() > 0)
        {
            cv::Mat mask(img.rows, img.cols, CV_8UC1, cv::Scalar(0));
            cv::fillPoly(mask, mask_area, cv::Scalar(255));
            img.copyTo(dst, mask);
        }
        return dst;
    }

    // 计算规划路径的轮廓
    std::unordered_map<int, std::vector<std::vector<cv::Point>>>
	    MapProcess::ExtractPlanPathContours(std::vector<std::vector<std::pair<float, float>>>& paths, bool flag)
    {
        std::vector<std::vector<cv::Point>> res_contours;
        cv::Mat path_img = cv::Mat::zeros(__map_Grid.rows, __map_Grid.cols, CV_8UC1);
	    for(auto path : paths){
            std::vector<cv::Point> mask_points;
            for(auto i = 0; i < path.size(); i++){
                cv::Point p = cv::Point(std::round((path[i].first - __origin_X) / __resolution),
                                        std::round((path[i].second - __origin_Y) / __resolution));
                mask_points.push_back(p);
            }
            if(mask_points.size() > 0) cv::polylines(path_img, mask_points, flag, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
	    }
        int sz = (int)std::round(((float)__brush_Length) / __resolution);
        cv::Mat erode_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(sz, sz), cv::Point(-1, -1)); // size: robot radius
        cv::dilate(path_img, path_img, erode_kernel);
        // 4.提取轮廓
        std::vector<std::vector<cv::Point>> cnts;
        std::vector<cv::Vec4i> hierarchy; // index: next, prev, first_child, parent
        cv::findContours(path_img, cnts, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
        // 5.判断规划区域是否存在轮廓
	    std::unordered_map<int, std::vector<std::vector<cv::Point>>> res_cnts;
        if(cnts.size() == 0 || hierarchy.size() == 0) return res_cnts;
        // 6.提取所有父类的轮廓
        std::vector<int> p_idxs;
        for(auto i = 0; i < hierarchy.size(); i++){
            if(hierarchy[i][3] == -1 && cnts[i].size() > 2 &&
                cv::arcLength(cnts[i], true) >= 10.0){
               p_idxs.emplace_back(i);
            }
        }
        // 7.父类包含的内部轮廓
        for(auto i = 0; i < p_idxs.size(); i++){
            std::vector<std::vector<cv::Point>> tmp_cnts;
            tmp_cnts.emplace_back(cnts[p_idxs[i]]);
            for(auto j = 0; j < hierarchy.size(); j++){
                if(p_idxs[i] == hierarchy[j][3]){
                    tmp_cnts.emplace_back(cnts[j]);
                }
            }
            res_cnts[i] = tmp_cnts;
        }

        return res_cnts;

    }

    // 计算轮廓区域的面积
    float MapProcess::CalcPlanArea(std::vector<std::vector<cv::Point>>& polys){
        float invalible_area = 0.;
        float all_area = 0.;
        for(auto i = 1; i < polys.size(); i++){
           invalible_area +=  cv::contourArea(polys[i]);
        }
        all_area += (cv::contourArea(polys[0]) - invalible_area) *
                                        __resolution * __resolution;
        return all_area;
    }

    // 规划区域内地图进行膨胀
    // 1.根据最外层的轮廓计算外接最小矩形
    // 2.根据矩形的宽度是否在窄道范围内使用不同的膨胀参数
    std::pair<int, cv::Mat> MapProcess::PlanAreaErode(int &piexl_radius, const cv::Mat &img)
    {
        // 根据膨胀查找规划区域的边界轮廓
        cv::Mat tmp_img = img.clone();
        std::vector<std::vector<cv::Point>> cnts;
        std::vector<cv::Vec4i> hierarchy; // index: next, prev, first_child, parent
        cv::findContours(tmp_img, cnts, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
	    if(cnts.size() == 0 || hierarchy.size() == 0) return std::make_pair(0, tmp_img);
        // 轮廓进行排序
        size_t max_area_idx = 0;
        double max_area = cv::contourArea(cnts[0]);
        for(size_t i = 1; i < cnts.size(); i++){
            auto area = cv::contourArea(cnts[i]);
            if(area > max_area){
                max_area = area;
                max_area_idx = i;
            }
        }
        // 获取轮廓的最小矩形
        int sz = piexl_radius;
        int narrow = 0;
        auto rotated_rect = cv::minAreaRect(cnts[max_area_idx]);
        if ((rotated_rect.size.width >= 14 && rotated_rect.size.width <= 23) ||
            (rotated_rect.size.height >= 14 && rotated_rect.size.height <= 23))
        { // TODO: rj.wang, 适配80的尺寸
            sz = 10; // 窄道特殊处理
            narrow = 1;
            LOG(INFO) << "narrow case, (width, height) is (" << rotated_rect.boundingRect2f().width << ", " << rotated_rect.boundingRect2f().height << ")";
        }
        cv::Mat erode_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(sz, sz), cv::Point(-1, -1)); // size: robot radius
        // cv::morphologyEx(img_, img_, cv::MORPH_ERODE, erode_kernel);
        cv::erode(tmp_img, tmp_img, erode_kernel);
        // // 应用中值滤波
        // cv::medianBlur(img, img, 3);
        return std::make_pair(narrow, tmp_img);
    }

    // 处理虚拟墙
    void MapProcess::ProcessVirtueWalls(cv::Mat &map_grid)
    {
        std::vector<std::vector<cv::Point>> virtue_wall_pixels;
        // 线段虚拟墙
        auto virtue_walls = __map_Data_Ptr->GetLineVirtueWalls();
        for (auto &virtue_wall : virtue_walls)
        {
            auto line_wall_pts = LineProcess(virtue_wall);
            if(line_wall_pts.size() < 1) continue;
            for(auto i = 0; i < line_wall_pts.size(); i++){
                map_grid.at<uchar>(line_wall_pts[i].y, line_wall_pts[i].x) = 0;
            }
        }
        // 圆形虚拟墙
        auto circle_virtue_walls = __map_Data_Ptr->GetCircleVirtueWalls();
        for (auto &circle_virtue_wall : circle_virtue_walls)
        {
            auto circle_wall_pts = CircleProcess(circle_virtue_wall);
            if(circle_wall_pts.size() < 1) continue;
            for (auto i = 0; i < circle_wall_pts.size(); i++)
            {
                map_grid.at<uchar>(circle_wall_pts[i].y, circle_wall_pts[i].x) = 0;
            }
        }
        // 矩形虚拟墙
        virtue_walls = __map_Data_Ptr->GetRectVirtueWalls();
        for (auto &virtue_wall : virtue_walls)
        {
            auto rect_wall_pts = LineProcess(virtue_wall);
            if(rect_wall_pts.size() < 1) continue;
            for(auto i = 0; i < rect_wall_pts.size(); i++){
                map_grid.at<uchar>(rect_wall_pts[i].y, rect_wall_pts[i].x) = 0;
            }
        }
        // 多边形虚拟墙
        virtue_walls = __map_Data_Ptr->GetPolyVirtueWalls();
        for (auto &virtue_wall : virtue_walls)
        {
            auto poly_wall_pts = LineProcess(virtue_wall);
            if(poly_wall_pts.size() < 1) continue;
            for(auto i = 0; i < poly_wall_pts.size(); i++){
                map_grid.at<uchar>(poly_wall_pts[i].y, poly_wall_pts[i].x) = 0;
            }
        }
    }

    // 虚拟墙或障碍物内部填充黑色表示不可达
    void MapProcess::FillNoConnectedArea(cv::Mat &map_grid)
    {
        // 提取特征点
        std::vector<std::vector<cv::Point>> cnts;
        std::vector<cv::Vec4i> hierarchy; // index: next, prev, first_child, parent
        cv::findContours(map_grid, cnts, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
        if(cnts.size() == 0 || hierarchy.size() == 0){
            return ;
        }
        // 孔洞子轮廓内部涂黑
        for(size_t i = 0; i < hierarchy.size(); i++){
            if(hierarchy[i][3] == -1 && hierarchy[i][2] != -1) { // 无父有子的最外轮廓i(内白)
                int contour_index = hierarchy[i][2];
                while (contour_index != -1) { // 轮廓c（内黑）, i的子
                    int hole_index = hierarchy[contour_index][2];
                    while (hole_index != -1) { // 孔洞h（内白）, c的子
                        cv::drawContours(map_grid, cnts, hole_index, cv::Scalar(0), cv::FILLED);
                        hole_index = hierarchy[hole_index][0]; // h同级的下一个孔洞
                    }
                    contour_index = hierarchy[contour_index][0]; // c同级的下一个轮廓
                }
            }
        }
    }

    // 处理功能区
    std::vector<std::pair<int, std::vector<cv::Point>>> MapProcess::ProcessFuncAreas()
    {
        std::vector<std::pair<int, std::vector<cv::Point>>> func_areas_pixels;
        // 矩形
        auto rect_func_areas = __map_Data_Ptr->GetRectFuncAreas();
        for (auto rect_func_area : rect_func_areas)
        {
            auto rect_func_area_piexl = LineProcess(rect_func_area.second);
            func_areas_pixels.emplace_back(std::make_pair(rect_func_area.first, rect_func_area_piexl));
        }
        // 圆形
        auto circle_func_areas = __map_Data_Ptr->GetCircleFuncAreas();
        for (auto circle_func_area : circle_func_areas)
        {
            auto circle_func_area_piexl = CircleProcess(circle_func_area.second);
            func_areas_pixels.emplace_back(std::make_pair(circle_func_area.first, circle_func_area_piexl));
        }
        // 多边形
        rect_func_areas = __map_Data_Ptr->GetPolyFuncAreas();
        for (auto rect_func_area : rect_func_areas)
        {
            auto rect_func_area_piexl = LineProcess(rect_func_area.second);
            func_areas_pixels.emplace_back(std::make_pair(rect_func_area.first, rect_func_area_piexl));
        }
        return func_areas_pixels;
    }

    // 处理规划外围区域和内部不可达区域
    std::vector<cv::Point> MapProcess::ProcessPolygons(geometry_msgs::Polygon &polygon)
    {
        std::vector<cv::Point> polygon_pixels;
        for (auto it = polygon.points.begin(); it != polygon.points.end(); it++)
        {
            cv::Point p = cv::Point(std::round((it->x - __origin_X) / __resolution),
                                    std::round((it->y - __origin_Y) / __resolution));
            polygon_pixels.push_back(p);
        }
        return polygon_pixels;
    }

    // 线段处理
    std::vector<cv::Point> MapProcess::LineProcess(std::vector<std::pair<float, float>> &line)
    {
        std::vector<cv::Point> line_pixels;
        for (auto i = 0; i < line.size() - 1; i += 2)
        {
            float dx, dy;
			dx = line[i + 1].first - line[i].first;
			dy = line[i + 1].second - line[i].second;
			float dis = sqrt(dx * dx + dy * dy);
			int point_num = std::ceil(dis / __resolution);
			bool is_singular = false;
			float k = 0.;
			float b = 0.;
			float l_interval = 0.;
			if (std::abs(dx) < 1e-6)
			{
				is_singular = true;
				l_interval = dy / (float)point_num;
			}
			else
			{
				k = dy / dx;
				b = line[i].second - k * line[i].first;
				l_interval = dx / (double)point_num;
			}
			for (int j = 0; j < point_num; j++)
			{
				float curX = 0.;
				float curY = 0.;
				if (is_singular)
				{
					curX = line[i].first;
					curY = line[i].second + j * l_interval;
				}
				else
				{
					curX = line[i].first + j * l_interval;
					curY = k * curX + b;
				}
				if (j == point_num - 1)
				{
					curX = line[i + 1].first;
					curY = line[i + 1].second;
				}

                int pixelx = std::round((curX - __origin_X) / __resolution);
                int pixely = std::round((curY - __origin_Y) / __resolution);
                if (pixelx >= 0 && pixely >= 0 &&
                    pixelx < __width && pixely < __height)
                {
                    line_pixels.emplace_back(cv::Point(pixelx, pixely));
                }
			}
        }
        return line_pixels;
    }

    // 圆形处理
    std::vector<cv::Point> MapProcess::CircleProcess(std::pair<std::pair<float, float>, float> &circle)
    {
        std::vector<cv::Point> circle_pixels;
        float dth = __resolution / circle.second;
        float theta = 0;
        while (theta < 2 * M_PI)
        {
            double curX = circle.second * std::cos(theta) + circle.first.first;
            double curY = circle.second * std::sin(theta) + circle.first.second;
            int idy = (curY - __origin_Y) / __resolution;
            int idx = (curX - __origin_X) / __resolution;
            if (idx < __width && idx > 0 &&
                idy < __height && idy > 0)
                circle_pixels.emplace_back(cv::Point(idx, idy));
            theta = theta + dth;
        }
        return circle_pixels;
    }

} // namespace preprocess
} // namespace global_path_planner